
//*****************************************
//	Oscar Rodriguez Polo	c) 2007
//
//	PackMemManager.cpp
//
//	Pack MEMORY Manager for NANOSAT 1B
//
//*****************************************



#include <public/PackMemManager.h>
#include <public/PackMemManagerPackAlloc.h>
#include <public/crc_16.h>




#define PACK_MEM_MNG_PACK_SIZE_IN_WORDS 128


//+++++++++++++++++++++++++++++++++++++++++++++++++
//*************************************************
//				Pack Handler
//*************************************************
//+++++++++++++++++++++++++++++++++++++++++++++++++


//*******************************************************
//!Constructor
//******************************************************

CPackHandler::CPackHandler(){

	PackIndex0=0xFFFF;
	PackIndex1=0xFFFF;
	PackIndex2=0xFFFF;

}


//******************************************************
//SetIndex();
//******************************************************


inline void   CPackHandler::SetIndex(word16_t _PackIndex0
		, word16_t _PackIndex1
		, word16_t _PackIndex2){

	PackIndex0=_PackIndex0;
	PackIndex1=_PackIndex1;
	PackIndex2=_PackIndex2;
}

//******************************************************
//GetIndex();
//******************************************************


inline void   CPackHandler::GetIndex(word16_t &_PackIndex0
		, 	word16_t &_PackIndex1
		, word16_t &_PackIndex2){

	_PackIndex0=PackIndex0;
	_PackIndex1=PackIndex1;
	_PackIndex2=PackIndex2;
}

//******************************************************
//IsIndexInInterval();
//******************************************************

bool_t   CPackHandler::IsIndexInInterval(
		word16_t _FistPack
		, word16_t _LastPack){
	bool_t isInInterval=false;

	if(IsValid()){
		if((_FistPack<=PackIndex0)&&(PackIndex0<=_LastPack))
			isInInterval=true;
	}
	return isInInterval;
}

//******************************************************
//ReadWord16();
//******************************************************


bool_t   CPackHandler::ReadWord16(uint8_t _numWord
		, word16_t &_wordValue) {

	bool_t MEMCheckOK=false;

	if (IsValid()){

		_wordValue= *(RAM_MEM_PACKS_POOL
				+ PackIndex0*PACK_MEM_MNG_PACK_SIZE_IN_WORDS
				+ _numWord);
		MEMCheckOK=true;
	}
	return MEMCheckOK;
}

//******************************************************
//WriteWord16();
//******************************************************


bool_t  CPackHandler::WriteWord16(uint8_t _numWord
		, word16_t _wordValue){

	bool_t MEMCheckOK=false;

	if (IsValid()){

		*(RAM_MEM_PACKS_POOL+PackIndex0
				* PACK_MEM_MNG_PACK_SIZE_IN_WORDS
				+ _numWord)=_wordValue;

		MEMCheckOK=true;
	}

	return MEMCheckOK;

}



//******************************************************
//ReadWord16Num();
//******************************************************

//return false if current is not a valid package
/*
//  (_fistWord + _WordsCounter) <128, does not write extra words
 */

bool_t  CPackHandler::ReadWord16Num(uint8_t _firstWord
		, word16_t *_wordValues
		, uint8_t _WordsCounter){

	bool_t MEMCheckOK=false;

	byte_t index =_firstWord;
	byte_t limit =index+_WordsCounter;


	if (limit>128)
		limit=128;

	if(IsValid()){
		MEMCheckOK=true;
		while(index<limit){
			*(_wordValues+(index-_firstWord))
                						=* (RAM_MEM_PACKS_POOL
                								+ PackIndex0*PACK_MEM_MNG_PACK_SIZE_IN_WORDS
                								+ index);
			index++;
		}

	}
	return MEMCheckOK;


}

//******************************************************
//WriteWord16Num();
//******************************************************

//return false if current is not a valid package
/*
//  (_fistWord + _WordsCounter) <128, does not write extra words
 */

bool_t  CPackHandler::WriteWord16Num(uint8_t _firstWord
		, const word16_t *_wordValues
		, uint8_t _WordsCounter){

	bool_t MEMCheckOK=false;

	byte_t index =_firstWord;
	byte_t limit =index+_WordsCounter;


	if (limit>128)
		limit=128;

	if(IsValid()){
		MEMCheckOK=true;
		while(index<limit){
			* (RAM_MEM_PACKS_POOL
					+ PackIndex0*PACK_MEM_MNG_PACK_SIZE_IN_WORDS+ index)=
							*(_wordValues+(index-_firstWord));
			index++;
		}

	}
	return MEMCheckOK;


}



//******************************************************
//ReadByte();
//******************************************************
//return false if it is not a valid package
bool_t   CPackHandler::ReadByte(byte_t _numByte
		, byte_t &_byteValue) {

	bool_t auxOK;
	word16_t auxWord16;
	auxOK=ReadWord16(_numByte>>1, auxWord16);
	if(auxOK){

#ifdef  CONFIG_PLATFORM_IS_LITTLE_ENDIAN
		if (_numByte&0x01)  //Odd byte
			_byteValue=auxWord16>>8;
		else
			_byteValue=auxWord16;

#else
		if (_numByte&0x01)  //Odd byte
			_byteValue=auxWord16;
		else
			_byteValue=auxWord16>>8;

#endif

	}

	return auxOK;

}

//******************************************************
//WriteByte();
//******************************************************
//return false if it is not a valid package
bool_t   CPackHandler::WriteByte(byte_t _numByte
		, byte_t _byteValue){

	bool_t auxOK;
	word16_t auxWord16;
	word16_t byteToWord16=_byteValue;
	auxOK=ReadWord16(_numByte>>1, auxWord16);
	if(auxOK){
#ifdef  CONFIG_PLATFORM_IS_LITTLE_ENDIAN
		if (_numByte&0x01)  //Odd byte
			auxWord16=(auxWord16&0x00FF)|(byteToWord16<<8);
		else
			auxWord16=(auxWord16&0xFF00)|byteToWord16;

#else
		if (_numByte&0x01)  //Odd byte
			auxWord16=(auxWord16&0xFF00)|byteToWord16;
		else
			auxWord16=(auxWord16&0x00FF)|(byteToWord16<<8);


#endif
	}
	if (auxOK)
		auxOK=WriteWord16(_numByte>>1, auxWord16);

	return auxOK;


}


//******************************************************
//IsValid();
//******************************************************


bool_t  CPackHandler::IsValid() {

	bool_t MEMCheckOK=true;
	//IS THE LAST NODE IN THE POOL?

	MEMCheckOK=
			CheckMEMWord16(&PackIndex0,&PackIndex1,&PackIndex2);

	if(MEMCheckOK){

		if (PackIndex0>=RAM_MEM_PACKS_NUM
				||PackIndex1>=RAM_MEM_PACKS_NUM
				||PackIndex2>=RAM_MEM_PACKS_NUM)
			MEMCheckOK=false;
	}

	return MEMCheckOK;


}


//*******************************************************
//CheckCRC

/*
//  return true if CRC check of the packet is OK and false if not
//  Read Word16s, cannot use the pack as a byte array
//
 */

bool_t  CPackHandler::CheckCRC(byte_t _FirstByte
		, byte_t _NumBytes){

	word16_t pDataCRC;
	word16_t CalcCRC;
	byte_t AuxByte;

	CalculateCRC(_FirstByte,_NumBytes,CalcCRC);

	ReadByte(_FirstByte+_NumBytes,AuxByte);
	pDataCRC=AuxByte;
	ReadByte(_FirstByte+_NumBytes+1,AuxByte);
	pDataCRC=(pDataCRC<<8)|AuxByte;


	return (CalcCRC==pDataCRC);


}


//*******************************************************
//CalculateCRC

/*
//  Calculate CRC and return it in _CRC var
//  _numBytes (from the _firstByte )are used to Calculate the CRC
//
 */

void  CPackHandler::CalculateCRC(byte_t _FirstByte
		, byte_t _NumBytes, word16_t &_CRC){

	byte_t index;
	register word16_t tempCRC;
	byte_t top;
	byte_t auxByte;
	//Init with
	tempCRC=0xFFFF;

	for(index=0; index< _NumBytes;index++){

		top = tempCRC >> 8;
		ReadByte(_FirstByte+index,auxByte);
		top ^= auxByte;
		tempCRC = (tempCRC << 8)^CRCWord16Table[top];
	}

	_CRC=tempCRC;

}




//+++++++++++++++++++++++++++++++++++++++++++++++++
//*************************************************
//				Pack List Handler
//*************************************************
//+++++++++++++++++++++++++++++++++++++++++++++++++


//*******************************************************
//!Constructor
//******************************************************

CPackListHandler::CPackListHandler(){

	InitAsEmptyPackList();

}

//******************************************************
//InitAsEmptyPackList();
//******************************************************

void  CPackListHandler::InitAsEmptyPackList(){

	NodeListTailIndex0=0xFFFF;
	NodeListTailIndex1=0xFFFF;
	NodeListTailIndex2=0xFFFF;

	NodeListHeadIndex0=0xFFFF;
	NodeListHeadIndex1=0xFFFF;
	NodeListHeadIndex2=0xFFFF;

	NodeListCurrentEditIndex=0xFFFF;
	NodeListCurrentReadIndex=0xFFFF;

	PackCounter0=0;PackCounter1=0;PackCounter2=0;

}

//******************************************************
//CheckCRCCurrentReadPack();
//******************************************************


byte_t  CPackListHandler::CheckCRCCurrentReadPack(){


	byte_t CheckCRC_Result=0;
	CPackHandler auxPack;

	//Config the pack
	auxPack.SetIndex(NodeListCurrentReadIndex,NodeListCurrentReadIndex
			,NodeListCurrentReadIndex);

	//Call to Calculate CRC
	if(auxPack.CheckCRC(0,254))
		CheckCRC_Result=1;

	return CheckCRC_Result;

}

//******************************************************
//WriteCRCCurrentWritePack();
//******************************************************


void  CPackListHandler::WriteCRCCurrentEditPack(
		byte_t _firstByte
		, byte_t _numBytes){


	word16_t CalculatedCRC;
	CPackHandler auxPack;

	//Config the pack
	auxPack.SetIndex(NodeListCurrentEditIndex
			,NodeListCurrentEditIndex
			,NodeListCurrentEditIndex);

	//Call to Calculate CRC
	auxPack.CalculateCRC(_firstByte,_numBytes, CalculatedCRC);

	//Write Calculate CRC in the next 2 bytes
	//Use Byte because can be misaligned
	auxPack.WriteByte(_firstByte+_numBytes,(byte_t)(CalculatedCRC>>8));
	auxPack.WriteByte(_firstByte+_numBytes+1,(byte_t)CalculatedCRC);




}




//******************************************************
//SetHead();
//******************************************************

inline void  CPackListHandler::SetHead(
		word16_t _NodeListHeadIndex0
		,word16_t _NodeListHeadIndex1
		,word16_t _NodeListHeadIndex2){

	NodeListHeadIndex0=_NodeListHeadIndex0;
	NodeListHeadIndex1=_NodeListHeadIndex1;
	NodeListHeadIndex2=_NodeListHeadIndex2;
	ToHeadPack();
	
}

//******************************************************
//GetHead();
//******************************************************

inline void  CPackListHandler::GetHead(
		word16_t &_NodeListHeadIndex0
		,word16_t &_NodeListHeadIndex1
		,word16_t &_NodeListHeadIndex2){

	_NodeListHeadIndex0=NodeListHeadIndex0;
	_NodeListHeadIndex1=NodeListHeadIndex1;
	_NodeListHeadIndex2=NodeListHeadIndex2;

}

//******************************************************
//SetTail();
//******************************************************


inline void  CPackListHandler::SetTail(
		word16_t _NodeListTailIndex0,
		word16_t _NodeListTailIndex1,
		word16_t _NodeListTailIndex2){

	NodeListTailIndex0=_NodeListTailIndex0;
	NodeListTailIndex1=_NodeListTailIndex1;
	NodeListTailIndex2=_NodeListTailIndex2;
	ToHeadPack();

}
//******************************************************
//GetTail();
//******************************************************


void  CPackListHandler::GetTail(
		word16_t &_NodeListTailIndex0
		,word16_t &_NodeListTailIndex1
		,word16_t &_NodeListTailIndex2){

	_NodeListTailIndex0=NodeListTailIndex0;
	_NodeListTailIndex1=NodeListTailIndex1;
	_NodeListTailIndex2=NodeListTailIndex2;

}

//******************************************************
//AddToTail
//******************************************************

void  CPackListHandler::AddToTail(
		word16_t _newNodeListTailIndex0
		, word16_t _newNodeListTailIndex1
		, word16_t _newNodeListTailIndex2){

	*(RAM_MEM_PACKS_NODES0 + NodeListTailIndex0)=_newNodeListTailIndex0;
	*(RAM_MEM_PACKS_NODES1 + NodeListTailIndex1)=_newNodeListTailIndex0;
	*(RAM_MEM_PACKS_NODES2 + NodeListTailIndex2)=_newNodeListTailIndex0;

}

//******************************************************
//UpdateToNext
//******************************************************

void  CPackListHandler::UpdateToNext(word16_t &_NodeListIndex0
		, word16_t &_NodeListIndex1
		, word16_t &_NodeListIndex2){

	_NodeListIndex0=*(RAM_MEM_PACKS_NODES0 +_NodeListIndex0);
	_NodeListIndex1=*(RAM_MEM_PACKS_NODES1 +_NodeListIndex1);
	_NodeListIndex2=*(RAM_MEM_PACKS_NODES2 +_NodeListIndex2);

}


//******************************************************
//CheckHeadMEM();
//******************************************************


bool_t  CPackListHandler::CheckHeadMEM(){
	
	return CheckMEMWord16(&NodeListHeadIndex0
			,&NodeListHeadIndex1
			,&NodeListHeadIndex2);
}


//******************************************************
//CheckTailMEM();
//******************************************************

bool_t  CPackListHandler::CheckTailMEM(){
	
	return CheckMEMWord16(
			&NodeListTailIndex0
			,&NodeListTailIndex1
			,&NodeListTailIndex2);
	
}

//******************************************************
//AddPack();
//******************************************************

/*!
//  Always Insert One Pack in the Tail
// It does check if List is Empty

 */



bool_t  CPackListHandler::AddPack(
		CPackHandler &packHandler){

	bool_t MEMCheckOK=true;

	word16_t packIndex0;
	word16_t packIndex1;
	word16_t packIndex2;

	if (packHandler.IsValid()){

		// Extract Pack index
		packHandler.GetIndex(packIndex0,packIndex1,packIndex2);

		//Check if it is Empty
		if(!IsEmpty()){

			//Check Tail
			MEMCheckOK=CheckTailMEM();

			if(MEMCheckOK){ //MEM is OK

				//Add the pack to Tail

				AddToTail(packIndex0,packIndex1,packIndex2);

			}
		}else{

			SetHead(packIndex0,packIndex1,packIndex2);
			PackCounter0=0; //It will be added after
			PackCounter1=0;
			PackCounter2=0;
		}


		if(MEMCheckOK){ //MEM is OK

			//Set Tail

			SetTail(packIndex0,packIndex1,packIndex2);

			//The last one of the list points to 0xFFFF

			AddToTail(0xFFFF,0xFFFF,0xFFFF);

			PackCounter0++;
			PackCounter1++;
			PackCounter2++;

		}

		packHandler.SetIndex(0xFFFF,0xFFFF,0xFFFF);

	}else
		MEMCheckOK=false;


	return MEMCheckOK;

}

//******************************************************
//TakeOnePack
//******************************************************

//Always Take the Pack at the Head, if it is Empty, the pack will be not valid

bool_t  CPackListHandler::TakeOnePack(
		CPackHandler & _packHandler){

	bool_t MEMCheckOK=true;

	if(!IsEmpty()){

		//Check Head
		MEMCheckOK=CheckHeadMEM();

		if (MEMCheckOK){

			MEMCheckOK=CheckHeadMEM();

			if (MEMCheckOK){

				//Take the package from the Pool Head
				_packHandler.SetIndex(NodeListHeadIndex0
						,NodeListHeadIndex1
						,NodeListHeadIndex2);

				//Update the Pool Head
				UpdateToNext(NodeListHeadIndex0
						,NodeListHeadIndex1
						,NodeListHeadIndex2);

				//It was the last node in the list
				if (NodeListHeadIndex0==0xFFFF
						||NodeListHeadIndex1==0xFFFF
						||NodeListHeadIndex2==0xFFFF){

					InitAsEmptyPackList();

				}else{

					PackCounter0--;
					PackCounter1--;
					PackCounter2--;

				}
			}

		}
	}else{
		InitAsEmptyPackList();
		_packHandler.SetIndex(0xFFFF,0xFFFF,0xFFFF);

	}

	return MEMCheckOK;
}

//******************************************************
//ExtractPackages();
//******************************************************


//_Pack
bool_t  CPackListHandler::ExtractPackages(
		word32_t _PackAcks
		, byte_t &_PacksNumber
		, CPackListHandler & _packListDestiny){

	bool_t MEMCheckOK=true;
	byte_t packsCounter;
	byte_t extractPacksCounter;
	word32_t AuxPackAcks;

	AuxPackAcks=_PackAcks;
	//Temp List
	CPackListHandler auxList;

	//aux Pack
	CPackHandler auxPack;

	//Only work with 32 packages maximum
	if(_PacksNumber>32)
		packsCounter=32;
	else
		packsCounter=_PacksNumber;


	extractPacksCounter=0;

	//Only if this Pack is NOT Empty
	while(packsCounter&&(!IsEmpty())&&MEMCheckOK){

		packsCounter--;

		//Take the next Pack

		MEMCheckOK=TakeOnePack(auxPack);

		if(MEMCheckOK){

			if(AuxPackAcks&0x01){
				//It is Marked => To destiny
				extractPacksCounter++;
				MEMCheckOK=_packListDestiny.AddPack(auxPack);
			}else
				//Not, to auxList
				MEMCheckOK=auxList.AddPack(auxPack);

			//Get next Mark
			AuxPackAcks=AuxPackAcks>>1;
		}

	}

	if(MEMCheckOK){

		// If any packs was not marked, auxList has elements

		if(_PacksNumber!=extractPacksCounter){

			_PacksNumber=extractPacksCounter;

			//Insert the rest of the packs to auxList
			ExtractAllPackages(auxList);

			//Move all to this packsList
			auxList.ExtractAllPackages(*this);

		}

	}

	if(IsEmpty())
		InitAsEmptyPackList();


	return MEMCheckOK;

}


//******************************************************
//ExtractPackages();
//******************************************************


//_Pack
bool_t  CPackListHandler::ExtractPackages(
		word16_t &_PacksNumber
		, CPackListHandler & _packListDestiny){

	bool_t MEMCheckOK=true;
	byte_t extractPacksCounter;
	word16_t packsCounter=_PacksNumber;

	//aux Pack
	CPackHandler auxPack;


	extractPacksCounter=0;

	//Only if this Pack is NOT Empty
	while(packsCounter&&(!IsEmpty())&&MEMCheckOK){

		packsCounter--;

		//Take the next Pack

		MEMCheckOK=TakeOnePack(auxPack);

		if(MEMCheckOK){
			
			extractPacksCounter++;
			MEMCheckOK=_packListDestiny.AddPack(auxPack);
		}

	}

	if(IsEmpty())
		InitAsEmptyPackList();

	_PacksNumber=extractPacksCounter;

	return MEMCheckOK;

}


//******************************************************
//ExtractAllPackages();
//******************************************************

/*!
//  Always insert the extracted packs in the Tail
//  It does check if Pack List is Empty

 */

bool_t  CPackListHandler::ExtractAllPackages(
		CPackListHandler & _packListDestiny){

	bool_t MEMCheckOK=true;

	//Only if this Pack List is NOT Empty
	if(!IsEmpty()){

		MEMCheckOK=CheckHeadMEM();

		MEMCheckOK=MEMCheckOK&&CheckTailMEM();

		MEMCheckOK=MEMCheckOK&& _packListDestiny.CheckTailMEM();

		if (MEMCheckOK){ //if Check is OK, the Pack will be insert

			if(!_packListDestiny.IsEmpty()){
				//If destiny has packs

				_packListDestiny.AddToTail(NodeListHeadIndex0
						, NodeListHeadIndex1
						, NodeListHeadIndex2);

				_packListDestiny.ToHeadPack();


			}else{
				//If destiny is empty
				_packListDestiny.SetHead(NodeListHeadIndex0
						,NodeListHeadIndex1
						,NodeListHeadIndex2);

			}

			_packListDestiny.SetTail(NodeListTailIndex0
					,NodeListTailIndex1
					,NodeListTailIndex2);

			_packListDestiny.PackCounter0
			=_packListDestiny.PackCounter0+PackCounter0;
			_packListDestiny.PackCounter1
			=_packListDestiny.PackCounter1+PackCounter1;
			_packListDestiny.PackCounter2
			=_packListDestiny.PackCounter2+PackCounter2;


		}

	}

	//Empty this Pack List
	InitAsEmptyPackList();

	return MEMCheckOK;
	
}



//******************************************************
//ReadWord16();
//******************************************************

bool_t   CPackListHandler::ReadWord16(byte_t _numWord,
		word16_t &_wordValue) const {



	CPackHandler pack;
	
	GetCurrentReadPack(pack);
	
	return pack.ReadWord16(_numWord, _wordValue);

}


//******************************************************
//WriteWord16();
//******************************************************

bool_t  CPackListHandler::WriteWord16(byte_t _numWord,
		word16_t _wordValue){

	CPackHandler pack;
	
	GetCurrentEditPack(pack);
	
	return pack.WriteWord16(_numWord, _wordValue);
	
}


//******************************************************
//ReadWord16Num();
//******************************************************

//return false if current is not a valid package
/*
//  (_fistWord + _WordsCounter) <128, does not write extra words
 */

bool_t  CPackListHandler::ReadWord16Num(
		byte_t _firstWord
		,word16_t *_wordValues
		,byte_t _WordsCounter) const {


	CPackHandler pack;
	
	GetCurrentReadPack(pack);
	
	return pack.ReadWord16Num( _firstWord, _wordValues ,_WordsCounter) ;


}

//******************************************************
//WriteWord16Num();
//******************************************************

//return false if current is not a valid package
/*
//  (_fistWord + _WordsCounter) <128, does not write extra words
 */
bool_t  CPackListHandler::WriteWord16Num(
		byte_t _firstWord
		, const word16_t *_wordValues
		, byte_t _WordsCounter){


	CPackHandler pack;
	
	GetCurrentEditPack(pack);
	
	return pack.WriteWord16Num(_firstWord,_wordValues, _WordsCounter);
	

}

//******************************************************
//ReadByte();
//******************************************************
//return false if it is not a valid package
bool_t   CPackListHandler::ReadByte(byte_t _numByte
		, byte_t &_byteValue) const {



	CPackHandler pack;
	
	GetCurrentReadPack(pack);
	
	return pack.ReadByte(_numByte, _byteValue) ;


}

//******************************************************
//WriteByte();
//******************************************************
//return false if it is not a valid package
bool_t   CPackListHandler::WriteByte(byte_t _numByte
		, byte_t _byteValue){


	CPackHandler pack;
	
	GetCurrentEditPack(pack);
	
	return pack.WriteByte(_numByte, _byteValue);
	



}





//******************************************************
//ToHeadPack();
//******************************************************

void  CPackListHandler::ToHeadPack(){

	CheckHeadMEM();

	NodeListCurrentReadIndex=NodeListHeadIndex0;
	NodeListCurrentEditIndex=NodeListHeadIndex0;
}

//******************************************************
//NextEditPack();
//******************************************************

bool_t  CPackListHandler::NextEditPack(){

	bool_t existNext=false;


	if(!IsEmpty()){

		if(!IsLastEdit()){
			//Advance in the list
			NodeListCurrentEditIndex
			=*(RAM_MEM_PACKS_NODES0
					+ NodeListCurrentEditIndex);
			if(NodeListCurrentReadIndex
					!=NodeListCurrentEditIndex)
				existNext=true;
		}else
			NodeListCurrentEditIndex=NodeListHeadIndex0;
	}
	return existNext;

}

//******************************************************
//NextReadPack();
//******************************************************

bool_t  CPackListHandler::NextReadPack(){

	bool_t existNext=false;


	if(!IsEmpty()){
		if(!IsLastRead()){
			//Advance in the list
			NodeListCurrentReadIndex
			=*(RAM_MEM_PACKS_NODES0
					+ NodeListCurrentReadIndex);

			if(NodeListCurrentReadIndex
					!=NodeListCurrentEditIndex)
				existNext=true;
		}else
			NodeListCurrentReadIndex
			=NodeListHeadIndex0;
	}
	return existNext;

}



/**  \brief Get current Edit Pack
 * 	 \param reference of a packHandler where the current edit pack info
 * 	is copied
 *
 */

//******************************************************
//NextReadPack();
//******************************************************

void  CPackListHandler::GetCurrentEditPack(
		CPackHandler & pack)const{
	
	pack.PackIndex0=NodeListCurrentEditIndex;
	
	pack.PackIndex1=NodeListCurrentEditIndex;
	
	pack.PackIndex2=NodeListCurrentEditIndex;
	
	
}



//******************************************************
//GetCurrentReadPack

void  CPackListHandler::GetCurrentReadPack(
		CPackHandler & pack)const{
	
	pack.PackIndex0=NodeListCurrentReadIndex;
	
	pack.PackIndex1=NodeListCurrentReadIndex;
	
	pack.PackIndex2=NodeListCurrentReadIndex;
}


//******************************************************
//GetEditPackAndAdvance();
//******************************************************

//  Get currentEdit Pack and Advance CurrentEdit without
//  Check if it is the last or if CurrentEdit is equal to ReadEdit
//  It is used in IRQ Handlers for fast access.


#ifdef EMU_NANOSAT1B

void  CPackListHandler::GetEditPackAndAdvance(
		word16_t *& pPackMEM){

	if(NodeListCurrentEditIndex<RAM_MEM_PACKS_NUM){
		pPackMEM=RAM_MEM_PACKS_POOL
				+ NodeListCurrentEditIndex*PACK_MEM_MNG_PACK_SIZE_IN_WORDS;
		NodeListCurrentEditIndex=*(RAM_MEM_PACKS_NODES0
				+ NodeListCurrentEditIndex);
	}

}


//******************************************************
//GetReadPackAndAdvance();
//******************************************************

//  Get currentRead Pack and Advance CurrentRead without Check
//	if it is the last or if CurrentEdit is equal to ReadEdit
//  It is used in IRQ Handlers for fast access.

void  CPackListHandler::GetReadPackAndAdvance(
		word16_t *& pPackMEM){

	if(NodeListCurrentReadIndex<RAM_MEM_PACKS_NUM){
		pPackMEM=RAM_MEM_PACKS_POOL
				+NodeListCurrentReadIndex*PACK_MEM_MNG_PACK_SIZE_IN_WORDS;

		NodeListCurrentReadIndex=*(RAM_MEM_PACKS_NODES0
				+ NodeListCurrentReadIndex);
	}

}

#endif

//******************************************************
//IsLastEdit();
//******************************************************

bool_t  CPackListHandler::IsLastEdit(){

	bool_t last=false;
	if (*(RAM_MEM_PACKS_NODES0 + NodeListCurrentEditIndex)==0xFFFF)
		last=true;
	return last;

}

//******************************************************
//IsLastRead();
//******************************************************

bool_t  CPackListHandler::IsLastRead(){

	bool_t last=false;
	if (*(RAM_MEM_PACKS_NODES0 + NodeListCurrentReadIndex)==0xFFFF)
		last=true;
	return last;

}



//******************************************************
//IsEmpty();
//******************************************************

bool_t  CPackListHandler::IsEmpty() {

	int isEmptyCount=0;


	if((NodeListHeadIndex0==0xFFFF)|| (NodeListTailIndex0==0xFFFF))
		isEmptyCount++;

	if((NodeListHeadIndex1==0xFFFF)|| (NodeListTailIndex1==0xFFFF))
		isEmptyCount++;

	if((NodeListHeadIndex2==0xFFFF)|| (NodeListTailIndex2==0xFFFF))
		isEmptyCount++;

	return (isEmptyCount>=2);
}


//******************************************************
//CheckPackCounter();
//******************************************************

#ifdef EMU_NANOSAT1B
#define _PACK_NUMBER_TO_CHECK_LIMIT		0xFFFF

#else

#define _PACK_NUMBER_TO_CHECK_LIMIT		0xFFFF
#endif

word16_t  CPackListHandler::CheckPackCounter(){


	bool_t MEMCheckOK;
	word16_t auxPackCounter;
	word16_t auxPackCounterLimit;
	word16_t AuxNodeListHeadIndex0;
	word16_t AuxNodeListHeadIndex1;
	word16_t AuxNodeListHeadIndex2;
	
	MEMCheckOK=CheckMEMWord16(&PackCounter0,&PackCounter1,&PackCounter2);
	
	//init total Pack number 
	auxPackCounter=0;
	

	//limit of Pack number to check
	if(	PackCounter0<_PACK_NUMBER_TO_CHECK_LIMIT)
		auxPackCounterLimit=PackCounter0;
	else
		auxPackCounterLimit=_PACK_NUMBER_TO_CHECK_LIMIT;
	
	//Check Head MEM
	MEMCheckOK=MEMCheckOK&&CheckHeadMEM();

	//Get Head 	
	GetHead(AuxNodeListHeadIndex0, AuxNodeListHeadIndex1,AuxNodeListHeadIndex2);

	//Check until found tail 	
	while (MEMCheckOK
			&&(AuxNodeListHeadIndex0!=0xFFFF)
			&&(AuxNodeListHeadIndex1!=0xFFFF)
			&&(AuxNodeListHeadIndex2!=0xFFFF)
			&& (auxPackCounter< auxPackCounterLimit)){

		auxPackCounter++;

		UpdateToNext(AuxNodeListHeadIndex0
				,AuxNodeListHeadIndex1
				,AuxNodeListHeadIndex2);


		MEMCheckOK=CheckMEMWord16(&AuxNodeListHeadIndex0
				,&AuxNodeListHeadIndex1
				,&AuxNodeListHeadIndex2);

		
	}
	//if true, check error, update pack number
	if(auxPackCounter!= auxPackCounterLimit)
		SetPackCounter(auxPackCounter);	
	

	return PackCounter0;
}



//******************************************************
//GetPackCounter();
//******************************************************

word16_t  CPackListHandler::GetPackCounter(){


#ifdef EMU_NANOSAT1B
	word16_t prevPackCounter;
	prevPackCounter=PackCounter0;
#endif

	
	word16_t auxPackCounter;
	
	auxPackCounter=CheckPackCounter();
	
#ifdef EMU_NANOSAT1B

	if (auxPackCounter!=prevPackCounter)
		printf("ERROOOOOOR PACK Counter");

#endif


	return auxPackCounter;
	
}

//******************************************************
//Set PackCounter();
//******************************************************


void  CPackListHandler::SetPackCounter(word16_t _packCount){

	PackCounter0=_packCount;
	PackCounter1=_packCount;
	PackCounter2=_packCount;


}


//+++++++++++++++++++++++++++++++++++++++++++++++++
//*************************************************
//				PackNodePool
//*************************************************
//+++++++++++++++++++++++++++++++++++++++++++++++++


//******************************************************
//InitPackagesMemory();
//******************************************************


void  PackPool::InitPackagesMemory() {
	int countPackages;

	word16_t * NodeDirBase0=RAM_MEM_PACKS_NODES0;
	word16_t * NodeDirBase1=RAM_MEM_PACKS_NODES1;
	word16_t * NodeDirBase2=RAM_MEM_PACKS_NODES2;

	FreePackNodeListHead0=0;
	FreePackNodeListHead1=0;
	FreePackNodeListHead2=0;

	for (countPackages=0; countPackages<(RAM_MEM_PACKS_NUM-1)
	; countPackages++){
		*(NodeDirBase0+countPackages)= countPackages+1;
		*(NodeDirBase1+countPackages)= countPackages+1;
		*(NodeDirBase2+countPackages)= countPackages+1;
	}

	NodeDirBase0[RAM_MEM_PACKS_NUM-1]= 0xFFFF;
	NodeDirBase1[RAM_MEM_PACKS_NUM-1]= 0xFFFF;
	NodeDirBase2[RAM_MEM_PACKS_NUM-1]= 0xFFFF;

	FreePackNodeListTail0=RAM_MEM_PACKS_NUM-1;
	FreePackNodeListTail1=RAM_MEM_PACKS_NUM-1;
	FreePackNodeListTail2=RAM_MEM_PACKS_NUM-1;

}


//******************************************************
//CheckPackagesMemory();
//******************************************************

#ifdef EMULATION_FOR_TEST

bool_t  PackPool::CheckPackagesMemory() {

	word16_t countPackages;
	bool_t MEMCheckOK=true;

	for (countPackages=0; countPackages<RAM_MEM_PACKS_NUM; countPackages++){
		MEMCheckOK=MEMCheckOK&&CheckMEMWord16(
				RAM_MEM_PACKS_NODES0+countPackages
				, RAM_MEM_PACKS_NODES1+countPackages
				, RAM_MEM_PACKS_NODES2+countPackages);

	}

	return MEMCheckOK;

}

#endif

//******************************************************
//IsEmpty();
//******************************************************

bool_t  PackPool::IsEmpty() {

	int isEmptyCount=0;


	if(FreePackNodeListHead0==0xFFFF)
		isEmptyCount++;

	if(FreePackNodeListHead1==0xFFFF)
		isEmptyCount++;

	if(FreePackNodeListHead2==0xFFFF)
		isEmptyCount++;


	return (isEmptyCount>=2);
}

//******************************************************
//FreePackages();
//******************************************************

/*!
        It does not check if Pool is Empty because the Pool has always 
        one element at least
 */

#ifdef EMULATION_FOR_TEST

bool_t  PackPool::FreePackages(
		CPackListHandler & listHandler){

	bool_t MEMCheckOK=true;

	word16_t PackNodeListTail0,PackNodeListTail1,PackNodeListTail2;
	word16_t PackNodeListHead0,PackNodeListHead1, PackNodeListHead2;

	if (!listHandler.IsEmpty()){
		//Get the Heads of the PackList

		listHandler.GetHead(PackNodeListHead0,PackNodeListHead1
				,PackNodeListHead2);

		//Check the Tails of the PackList

		listHandler.GetTail(PackNodeListTail0,PackNodeListTail1
				,PackNodeListTail2);


		MEMCheckOK=CheckMEMWord16(&PackNodeListHead0
				,&PackNodeListHead1
				,&PackNodeListHead2);

		MEMCheckOK=MEMCheckOK&&CheckMEMWord16(&PackNodeListTail0
				,&PackNodeListTail1
				,&PackNodeListTail2);

		MEMCheckOK=MEMCheckOK&&CheckMEMWord16(&FreePackNodeListTail0
				,&FreePackNodeListTail1
				,&FreePackNodeListTail2);

		MEMCheckOK=MEMCheckOK&&CheckMEMWord16(&FreePackNodeListHead0
				,&FreePackNodeListHead1
				,&FreePackNodeListHead2);


		if (MEMCheckOK){ //if Check is OK, the Pack will be insert

			//IF it is not empty add to Tail
			if(!IsEmpty()){
				*(RAM_MEM_PACKS_NODES0 + FreePackNodeListTail0)
                    											=PackNodeListHead0;
				*(RAM_MEM_PACKS_NODES1 + FreePackNodeListTail1)
                    											=PackNodeListHead1;
				*(RAM_MEM_PACKS_NODES2 + FreePackNodeListTail2)
                    											=PackNodeListHead2;

			}else{
				//else set Head and Tail to the PackNodeList

				FreePackNodeListHead0=PackNodeListHead0;
				FreePackNodeListHead1=PackNodeListHead1;
				FreePackNodeListHead2=PackNodeListHead2;
				
			}

			FreePackNodeListTail0=PackNodeListTail0;
			FreePackNodeListTail1=PackNodeListTail1;
			FreePackNodeListTail2=PackNodeListTail2;


			listHandler.InitAsEmptyPackList();


		}
	}

	return MEMCheckOK;

}

#endif

//******************************************************
//AllocPackages();
//******************************************************


bool_t  PackPool::AllocPackages(word16_t &_numPackages
		, CPackListHandler & listHandler){

	word16_t count;
	word16_t actualNode0,actualNode1,actualNode2;

	bool_t MEMCheckOK=true;
	
	if(_numPackages){
		//EXTRACT FROM THE HEAD

		//Check FreeNodeListHeads
		MEMCheckOK=CheckMEMWord16(&FreePackNodeListHead0
				,&FreePackNodeListHead1
				,&FreePackNodeListHead2);

		if(!IsEmpty()){

			if(MEMCheckOK){

				actualNode0=FreePackNodeListHead0;
				actualNode1=FreePackNodeListHead1;
				actualNode2=FreePackNodeListHead2;

				count=1; //FreePackNodeListHead is the first Pack

				while(count<_numPackages&&MEMCheckOK){

					//Check the NEXT ONE
					MEMCheckOK=CheckMEMWord16(
							RAM_MEM_PACKS_NODES0+actualNode0
							, RAM_MEM_PACKS_NODES1+actualNode1
							, RAM_MEM_PACKS_NODES2+ actualNode2);

					//鿿IS THE LAST NODE IN THE POOL?

					if (*(RAM_MEM_PACKS_NODES0 + actualNode0)==0xFFFF
							||*(RAM_MEM_PACKS_NODES1 + actualNode1)==0xFFFF
							||*(RAM_MEM_PACKS_NODES2 + actualNode2)==0xFFFF){

						//Cannot allocate all the packs.
						_numPackages=count;

					}else {

						actualNode0=*(RAM_MEM_PACKS_NODES0 + actualNode0);
						actualNode1=*(RAM_MEM_PACKS_NODES1 + actualNode1);
						actualNode2=*(RAM_MEM_PACKS_NODES2 + actualNode2);

						count++;
					}

				};

			}else
				MEMCheckOK=false;
		}else
			_numPackages=0;


		//THE PACK LIST is allocated only if MEMCheckOK&&_numPackages>0
		if (MEMCheckOK&&(_numPackages>0)){

			listHandler.SetHead(FreePackNodeListHead0, FreePackNodeListHead1
					, FreePackNodeListHead2);

			listHandler.SetTail(actualNode0, actualNode1,actualNode2);

			FreePackNodeListHead0=*(RAM_MEM_PACKS_NODES0 + actualNode0);
			FreePackNodeListHead1=*(RAM_MEM_PACKS_NODES1 + actualNode1);
			FreePackNodeListHead2=*(RAM_MEM_PACKS_NODES2 + actualNode2);

			//POOL EMPTY
			if (FreePackNodeListHead0==0xFFFF
					||FreePackNodeListHead1==0xFFFF
					||FreePackNodeListHead2==0xFFFF){

				FreePackNodeListTail0=0xFFFF;
				FreePackNodeListTail1=0xFFFF;
				FreePackNodeListTail2=0xFFFF;

			}

			//End of the packlist
			*(RAM_MEM_PACKS_NODES0 + actualNode0)=0xFFFF;
			*(RAM_MEM_PACKS_NODES1 + actualNode1)=0xFFFF;
			*(RAM_MEM_PACKS_NODES2 + actualNode2)=0xFFFF;

			listHandler.SetPackCounter(_numPackages);

		}else{
			listHandler.InitAsEmptyPackList();

		}

	}else{

		listHandler.InitAsEmptyPackList();
		
	}

	return MEMCheckOK;

}

#ifdef  EMULATION_FOR_TEST

//******************************************************
//FreeOnePackage();
//******************************************************

/*!
        It does not check if Pool is Empty because the Pool has always 
        one element at least
 */

bool_t  PackPool::FreeOnePackage(
		CPackHandler & _packHandler){

	bool_t MEMCheckOK=true;
	word16_t AuxPackIndex0;
	word16_t AuxPackIndex1;
	word16_t AuxPackIndex2;

	if (_packHandler.IsValid()){

		_packHandler.GetIndex(AuxPackIndex0, AuxPackIndex1
				, AuxPackIndex2);

		//MEMORY Has been checked in IsValid

		//MEMCheckOK=CheckMEMWord16(&AuxPackIndex0, &AuxPackIndex1
		//											, &AuxPackIndex2);

		MEMCheckOK=MEMCheckOK&&CheckMEMWord16(&FreePackNodeListTail0
				,&FreePackNodeListTail1
				,&FreePackNodeListTail2);


		if (MEMCheckOK){

			if(IsEmpty()){

				FreePackNodeListHead0=AuxPackIndex0;
				FreePackNodeListHead1=AuxPackIndex1;
				FreePackNodeListHead2=AuxPackIndex2;

			}else{

				//Insert in the Tail

				*(RAM_MEM_PACKS_NODES0 + FreePackNodeListTail0)
                        											=AuxPackIndex0;
				*(RAM_MEM_PACKS_NODES1 + FreePackNodeListTail1)
                        											=AuxPackIndex1;
				*(RAM_MEM_PACKS_NODES2 + FreePackNodeListTail2)
                        											=AuxPackIndex2;


			}

			FreePackNodeListTail0=AuxPackIndex0;
			FreePackNodeListTail1=AuxPackIndex1;
			FreePackNodeListTail2=AuxPackIndex2;

			//Put 0xFFFF as the next node of the tail


			*(RAM_MEM_PACKS_NODES0 + FreePackNodeListTail0)=0xFFFF;
			*(RAM_MEM_PACKS_NODES1 + FreePackNodeListTail1)=0xFFFF;
			*(RAM_MEM_PACKS_NODES2 + FreePackNodeListTail2)=0xFFFF;

			_packHandler.SetIndex(0xFFFF, 0xFFFF, 0xFFFF);


		}
	}else
		MEMCheckOK=false;


	return MEMCheckOK;

}


//******************************************************
//AllocOnePackage();
//******************************************************

bool_t  PackPool::AllocOnePackage(
		CPackHandler & _packHandler){

	bool_t MEMCheckOK;

	MEMCheckOK=CheckMEMWord16(&FreePackNodeListHead0
			,&FreePackNodeListHead1
			,&FreePackNodeListHead2);

	if (MEMCheckOK){

		if(!IsEmpty()){

			MEMCheckOK=CheckMEMWord16(RAM_MEM_PACKS_NODES0
					+ FreePackNodeListHead0
					, RAM_MEM_PACKS_NODES1
					+ FreePackNodeListHead1,
					RAM_MEM_PACKS_NODES2
					+ FreePackNodeListHead2);

			if (MEMCheckOK){

				//Take the package from the Pool Head
				_packHandler.SetIndex(FreePackNodeListHead0
						,FreePackNodeListHead1
						,FreePackNodeListHead2);



				//Update the Pool Head
				FreePackNodeListHead0=*(RAM_MEM_PACKS_NODES0
						+ FreePackNodeListHead0);
				FreePackNodeListHead1=*(RAM_MEM_PACKS_NODES1
						+ FreePackNodeListHead1);
				FreePackNodeListHead2=*(RAM_MEM_PACKS_NODES2
						+ FreePackNodeListHead2);

			}

		}

	}

	return MEMCheckOK;
}
//******************************************************
//NumPacksFree();
//******************************************************



bool_t  PackPool::NumPacksFree(word16_t &numPacksFree){

	bool_t MEMCheckOK=true;
	word16_t countPackages=0;
	word16_t actualPack0,actualPack1,actualPack2;

	actualPack0=FreePackNodeListHead0;
	actualPack1=FreePackNodeListHead1;
	actualPack2=FreePackNodeListHead2;

	while(actualPack0!=0xFFFF&&actualPack1!=0xFFFF&&actualPack2!=0xFFFF){

		MEMCheckOK=MEMCheckOK&&CheckMEMWord16(RAM_MEM_PACKS_NODES0
				+ actualPack0
				, RAM_MEM_PACKS_NODES1 + actualPack1
				, RAM_MEM_PACKS_NODES2+ actualPack2 );

		actualPack0=*(RAM_MEM_PACKS_NODES0 + actualPack0);
		actualPack1=*(RAM_MEM_PACKS_NODES1 + actualPack1);
		actualPack2=*(RAM_MEM_PACKS_NODES2 + actualPack2);
		countPackages++;

	};


	numPacksFree=countPackages;

	return MEMCheckOK;


}

#endif

//+++++++++++++++++++++++++++++++++++++++++++++++++
//*************************************************
//				MEMORY CHECK
//*************************************************
//+++++++++++++++++++++++++++++++++++++++++++++++++


//******************************************************
//CheckMEMWord16();

bool_t  CheckMEMWord16(word16_t * _pword0
		, word16_t * _pword1
		, word16_t * _pword2){

	bool_t CheckResult=false;

	word16_t node0_1 =(*_pword0)^(*_pword1);
	word16_t node0_2 =(*_pword0)^(*_pword2);
	word16_t node1_2 =(*_pword1)^(*_pword2);

	//wort execution time for the positive check


	if(node0_1==0){
		(*_pword2)=(*_pword0);
		CheckResult=true;
	}


	if(node0_2==0){
		(*_pword1)=(*_pword0);
		CheckResult=true;
	}

	if(node1_2==0){
		(*_pword0)=(*_pword1);
		CheckResult=true;
	}

	return CheckResult;

}


//******************************************************
//GetMEMWord16();

bool_t  GetMEMWord16(word16_t &dataWord16
		, word16_t * &_pword0
		, word16_t * &_pword1
		, word16_t * &_pword2){

	bool_t CheckResult=false;
	word16_t node0_1 =(*_pword0)^(*_pword1);
	word16_t node0_2 =(*_pword0)^(*_pword2);
	word16_t node1_2 =(*_pword1)^(*_pword2);

	//wort execution time for the positive check

	if(node0_1==0){
		dataWord16=*_pword0;
		CheckResult=true;
	}

	if(node0_2==0){
		dataWord16=*_pword0;
		CheckResult=true;
	}
	if(node1_2==0){
		dataWord16=*_pword1;
		CheckResult=true;

	}

	return CheckResult;

}


//TODO: probar con el const reference
//void PacksToBytes(const CPackListHandler & packets, byte_t * pBytes)
void PacketsToBytes(CPackListHandler packets, byte_t * pBytes)
{
	CPackHandler packAux;
	uint16_t byteIndex=32;
	uint16_t packIndex;

	packets.ToHeadPack();
	if (packets.GetPackCounter())
	{
		do
		{
			packets.GetCurrentReadPack(packAux);
			for (packIndex =0; packIndex < 256; packIndex++)
			{
				packAux.ReadByte(packIndex,pBytes[byteIndex++]);
			}
		}
		while (packets.NextReadPack());
	}

}














